#include "ccloth.h"

#include <buola/cv/ccloud.h>
#include <buola/scene/geodes/cplane.h>
#include <buola/scene/geodes/ccube.h>
#include <buola/scene/transform/csoftbody.h>
#include <buola/scene/cscene.h>
#include <buola/iterator/transform.h>
#include <buola/algorithm/range.h>
#include <buola/image/algorithm/structure.h>

#include <BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h>
#include <BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h>

namespace buola { namespace tracker {

static const float sKPCloth=1;
    
struct AClothFilter
{
    bool operator()(const cvi::CCloud_rgb8::TPoint &pP) const
    {
        if(pP.mPixel[0]>150||pP.mPixel[2]<150) return false;
        if(pP.mPixel[2]<=pP.mPixel[0]+30||pP.mPixel[1]<=pP.mPixel[0]+30) return false;
        if(pP.mPoint.x()<-0.45f) return false;
        if(pP.mPoint.x()>0.3f) return false;
        if(pP.mPoint.y()<0.6f) return false;
        if(pP.mPoint.y()>2.0f) return false;
//         if(pP.mPoint.z()>-0.7) return false;
        return true;
    }
};

CCloth::CCloth(const cvi::CCloud_rgb8 &pCloud)
{
    auto lFiltered=cvi::filter(pCloud,AClothFilter());

    typedef typename cvi::CCloud_rgb8::TPoint TPoint;
    auto lMedian=median(lFiltered,[](const TPoint &p1,const TPoint &p2){return p1.mPoint.z()<p2.mPoint.z();});
    auto lMin=min_value(lFiltered,[](const TPoint &p1,const TPoint &p2){return p1.mPoint.z()<p2.mPoint.z();});
    
    std::vector<CPoint_f> lPoints;
    for(int i=0;i<lFiltered.size();i++)
    {
        lPoints.emplace_back(lFiltered[i].mPoint.x(),lFiltered[i].mPoint.y());
    }
    auto lCorners2D=img::min_area_rect(lPoints);
    msg_info() << "corners\n";
    std::array<mat::CVec3f,4> lCorners3D;
    for(int i=0;i<4;i++)
    {
        mat::CVec3f lCorner(lCorners2D[i].x,lCorners2D[i].y,lMedian.mPoint.z());
        auto lMin=min_element(lFiltered,[&lCorner](const TPoint &p1,const TPoint &p2)
                                      {return distance(p1.mPoint,lCorner)<distance(p2.mPoint,lCorner);});
        lCorners3D[i]=lMin->mPoint;
    }
    
    mSoftBody=new scene::CSoftBody(new scene::CCloth(lCorners3D,1.0,15,15));

    mPlane=new scene::CRigidBody(new scene::CCube(5,5,1),0,mat::translation_3d({0,0,lMin.mPoint.z()-0.5f}));
}

int CCloth::GetK()
{
    return mSoftBody->GetBulletBody()->m_nodes.size();
}

cvi::CCloud_rgb8 CCloth::FilterCloud(const cvi::CCloud_rgb8& pCloud)
{
    return cvi::filter(pCloud,AClothFilter());
}

mat::CMat_f CCloth::GetCloudFeatures(const cvi::CCloud_rgb8& pCloud)
{
    mat::CMat_f lResult(pCloud.size()/20,3);
    
    for(int i=0;i<lResult.Rows();i++)
    {
        lResult(i,nAll)=pCloud[i*20].mPoint.T();
    }
    
    return lResult;
}

mat::CMat_f CCloth::GetObjectFeatures()
{
    btSoftBody *lBody=mSoftBody->GetBulletBody().get();
    
    mat::CMat_f lResult(lBody->m_nodes.size(),3);
    for(int i=0;i<lBody->m_nodes.size();i++)
    {
        const auto &lVec=lBody->m_nodes[i].m_x;
        lResult(i,nAll)=mat::CVec3f{lVec[0],lVec[1],lVec[2]};
    }
    
    return lResult;
}

void CCloth::ApplyEvidence(const mat::CMat_f &pPZGivenC,const mat::CMat_f &pObserved,const mat::CMat_f &pObject)
{
    btSoftBody *lBody=mSoftBody->GetBulletBody().get();
    
    for(int k=0;k<lBody->m_nodes.size();k++) 
    { 
        btVector3 lImpulse(0,0,0);//=-sKDRope*lBody->GetBulletBody()->getLinearVelocity();
        for(int n=0;n<pPZGivenC.Cols();n++)
        {
            btVector3 lObsPt(pObserved(n,0),pObserved(n,1),pObserved(n,2));
            btVector3 lEstPt(pObject(k,0),pObject(k,1),pObject(k,2));
            lImpulse+=(sKPCloth*pPZGivenC(k,n))*(lObsPt-lEstPt);
        }
        
        lBody->addForce(lImpulse,k);
    }
}

void CCloth::AddToScene(const scene::PScene &pScene)
{
    pScene->World()->AddChild(mSoftBody);
    pScene->AddBody(mSoftBody);
    
    pScene->World()->AddChild(mPlane);
    pScene->AddBody(mPlane);
}

/*namespace tracker*/ } /*namespace buola*/ }
